PID控制器代码实现
//定义pid结构体
struct PID
{
	float kp;
	float ki;
	float kd;
};
typedef struct PID _PID;

//定义转向pid参数结构体
_PID TurnPdate=
{
	.kp=100,			//赋值比例值60
	.ki=0,			//赋值积分值
	.kd=-34			//赋值微分值-45
};		
//定义速度pid参数结构体
_PID SpdPdate=
{
	.kp=-50,			//赋值比例值-80
	.ki=-6,			//赋值积分值-10
	.kd=0			//赋值微分值
};	

/*@brief: 位置式PID控制器
 * @param:
 *        [in] float deviation: 和目标值得偏差
 *        [in] _PID pid: 位置式pid的参数
 * @return: 调节占空比的一个整形数
 */
int PositionPID(float deviation,_PID pid)
{
	float Position_KP=pid.kp,Position_KI=pid.ki,Position_KD=pid.kd;
	static float Bias,Pwm,Integral_bias,Last_Bias;
	Bias=deviation;                         		         //计算偏差
	Integral_bias+=Bias;	                                 //求出偏差的积分
	Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
	Last_Bias=Bias;                                      	 //保存上一次偏差 
	return Pwm;    
}

/*@brief: 为速度pid使用位置式PID控制器，对积分值进行限幅
 * @param:
 *        [in] float deviation: 和目标值得偏差
 *        [in] _PID pid: 位置式pid的参数
 * @return: 调节占空比的一个整形数
 */
int PositionPIDToSpd(float deviation,_PID pid)
{
	float Position_KP=pid.kp,Position_KI=pid.ki,Position_KD=pid.kd;
	static float Bias,Pwm,Integral_bias,Last_Bias,pwmKI=0;
	Bias=deviation;                         		         //计算偏差
	Integral_bias+=Bias;	                                 //求出偏差的积分
	pwmKI=Position_KI*Integral_bias;
	if(pwmKI>MAX_MOTOR_PWM) Integral_bias=MAX_MOTOR_PWM/Position_KI;
	Pwm=Position_KP*Bias+pwmKI+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
	Last_Bias=Bias;                                      	 //保存上一次偏差 
	return Pwm;    
}


//差速转向车型，这种车转向方法是通过控制车轮的差速
/*@brief:根据循迹传感器pid调节小车转向使小车处于黑线中间
 * @param:
 *        [in]int TraceDate: 循迹传感器输出的值
 *        [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
 * @return: 返回调节电机速度的转向pwm
 */
int ChangeTraceTurn(int TraceDate)
{
	int pwm=0;
	int bias;
	bias=TraceDate;
	pwm=PositionPID(bias,TraceTurnPdate);
	if(pwm>5000)		pwm=5000;//限幅
	else if(pwm<-5000)  pwm=-5000;
	return pwm;
}

/*@brief:根据pid调节左边电机到目标速度
 * @param:
 *        [in]int EncodeSpdL: 当前左电机编码器测速值
 *        [in]float TarSpdL:左边电机目标速度,最大速度越1.19m/s
 * @return: 返回左边电机计算后的pwm占空比
 */
int ChangeSpeedMotorL(int NowEncodeSpdL,float TarSpdL)
{
	int pwm=0;
	int bias;
	int TarEncodeSpdL;
	TarEncodeSpdL=(int)((TarSpdL*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
	bias=NowEncodeSpdL-TarEncodeSpdL;
	pwm=PositionPIDToSpd(bias,SpdPdate);
	if(pwm>5000)		pwm=5000;//限幅
	else if(pwm<-5000)  pwm=-5000;
	return pwm;
}

/*@brief:根据pid调节右边电机到目标速度
 * @param:
 *        [in]int EncodeSpdL: 当前右电机编码器测速值
 *        [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
 * @return: 返回右边电机计算后的pwm占空比
 */
int ChangeSpeedMotorR(int NowEncodeSpdR,float TarSpdR)
{
	int pwm=0;
	int bias;
	int TarEncodeSpdR;
	TarEncodeSpdR=(int)((TarSpdR*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
	bias=NowEncodeSpdR-TarEncodeSpdR;
	pwm=PositionPIDToSpd(bias,SpdPdate);
	if(pwm>5000)		pwm=5000;//限幅
	else if(pwm<-5000)  pwm=-5000;
	return pwm;
}

/*@brief: 让小车根据循迹黑线走
 *@param:
 *        [in]TraceDate: 循迹传感器输出的值
 *        [in]TarSpeed:循迹的目标速度
 *@return: 到达目标点返回1，否则返回0
 */
void TraceMove(int TraceDate,float TarSpeed)
{
	int turnpwm=0;
	int spdpwml=0,spdpwmr=0;
	int pwml=0,pwmr=0;
	
	turnpwm=ChangeTraceTurn(TraceDate);
	
	spdpwml=ChangeSpeedMotorL(Encode_Left,TarSpeed);
	spdpwmr=ChangeSpeedMotorR(Encode_Right,TarSpeed);
	
	
	pwmr=turnpwm+spdpwmr;
	if(pwmr>5000)		pwmr=5000;//限幅
	else if(pwmr<-5000)  pwmr=-5000;
	pwml=-turnpwm+spdpwml;
	if(pwml>5000)		pwml=5000;//限幅
	else if(pwml<-5000)  pwml=-5000;
	
	Set_Pwm_Motor1(pwmr);				//设置电机1占空比
	Set_Pwm_Motor2(pwml);				//设置电机2占空比
}
